
#include "glodef.h"
#include "errlog.h"
#include "grid3d.h"
#include "fe_matr.h"
#include "glovar.h"

#include "soah_common.h"
#include "vecs.h"
#include <string.h>
#include <stdio.h>
#include <math.h>

static int tensor_N(const face_t *f, const elem_t *T, double *TN, double *n)
/*
 * computes
 *      N[i][j+5*b][k+5*c] = int_f\hat{phi}_i \partial_b\phi_j\partial_c\phi_k
 * where
 *      \hat{\phi}_i, i=0,1,2 are the P_1 basis on face f
 *      \phi_j, j,k=0,1,2,3,4 are the P_1-bubble basis in element T
 *      x_b, b,c=0,1,2 are the spatial variables
 */
{
    int i, j, k, b, c, s;
    double V, A[4], An[4][3], As_by_V2;

#define index_N(i,j,k)     ((i)*15*15 + (j)*15 + (k))

    if(TN == NULL)
        return -1;

    V = ComputeVolume(grid->nodes, T->nodes);
    for(i = 0; i < 4; i++)
    {
        A[i] = ComputeArea(grid->nodes,
                elem_face_nodes(T->nodes, i), n + i * 3);
        for(j = 0; j < 3; j++) An[i][j] = A[i]*(n + i * 3)[j];
    }

    /* f must be a face in T,  s is the local index of f in T */
    s = 0 + 1 + 2 + 3;
    for(i = 0; i < 3; i++)
    {
        for(j = 0; j < 4; j++)
            if(f->nodes[i] == T->nodes[j])
            {
                s -= j;
                break;
            }
        if(j >= 4)
            error_msg("%s: face %ld is not in element %ld.\n", __func__,
                f - grid->faces, T - grid->elems);
    }
    As_by_V2 = A[s] / V / V;

    memset(TN, 0, 3 * 15 * 15 * sizeof (double));
    for(i = 0; i < 3; i++)
        for(b = 0; b < 3; b++)
            for(c = 0; c < 3; c++)
            {
                const double C1 = 1. / 27.;
                const double C2 = 64. / 405.;
                const double C4 = 8192. / 8505.;
                double Val;

                /* case 1: j<4, k<4 */
                for(j = 0; j < 4; j++)
                    for(k = 0; k < 4; k++)
                    {
                        Val = As_by_V2 * C1 * An[j][b] * An[k][c];
                        TN[index_N(i, j + 5 * b, k + 5 * c)] = Val;
                    }
                /* case 2: j=4, k<4 */
                for(k = 0; k < 4; k++)
                {
                    Val = As_by_V2 * C2 * An[s][b] * An[k][c];
                    TN[index_N(i, 4 + 5 * b, k + 5 * c)] = Val;
                }
                /* case 3: j<4, k=4 */
                for(j = 0; j < 4; j++)
                {
                    Val = As_by_V2 * C2 * An[s][c] * An[j][b];
                    TN[index_N(i, j + 5 * b, 4 + 5 * c)] = Val;
                }
                /* case 4: j=4, k=4 */
                Val = As_by_V2 * C4 * An[s][b] * An[s][c];
                TN[index_N(i, 4 + 5 * b, 4 + 5 * c)] = Val;
            }
#undef index_N
    return s;
}

/****************************************************************************/

static int prep_face_data(const face_t *f,
        const vector Z, const vector U, const vector V,
        double *z, double *u, double *v,
        double *TN, double *n)
{
    int j, i, Tn = f->elems[0];
    elem_t *T = grid->elems + Tn;
    assert(f != NULL && Z != NULL && U != NULL && V != NULL &&
            z != NULL && u != NULL && v != NULL && n != NULL && TN != NULL);

    use_vector(U)
    use_vector(V)
    use_vector(Z)
    for(j = 0; j < 4; j++)
    {
        (u + j * 3)[0] = UX[T->nodes[j]];
        (v + j * 3)[0] = VX[T->nodes[j]];
        (u + j * 3)[1] = UY[T->nodes[j]];
        (v + j * 3)[1] = VY[T->nodes[j]];
        (u + j * 3)[2] = UZ[T->nodes[j]];
        (v + j * 3)[2] = VZ[T->nodes[j]];
    }
    (u + 4 * 3)[0] = UX[grid->NN + Tn];
    (v + 4 * 3)[0] = VX[grid->NN + Tn];
    (u + 4 * 3)[1] = UY[grid->NN + Tn];
    (v + 4 * 3)[1] = VY[grid->NN + Tn];
    (u + 4 * 3)[2] = UZ[grid->NN + Tn];
    (v + 4 * 3)[2] = VZ[grid->NN + Tn];

    for(i = 0; i < 3; i++)
    {
        (z + i * 3)[0] = ZX[f->nodes[i]];
        (z + i * 3)[1] = ZY[f->nodes[i]];
        (z + i * 3)[2] = ZZ[f->nodes[i]];
    }
    done_vector(Z)
    done_vector(V)
    done_vector(U)

    return tensor_N(f, T, TN, n);
}

inline static int check_inputs_U(const vector Z, const vector U,
        const vector V, scalar S)
{
    assert(Z != NULL && S != NULL);
    assert(get_N_vec(Z) == grid->NN);
    assert(get_N_scal(S) == grid->NN);
    clean_scal(S);
    if(U == NULL || V == NULL)
        return 0;
    assert(get_N_vec(U) == fe_ndof(grid, fe_bub));
    assert(get_N_vec(V) == fe_ndof(grid, fe_bub));
    return 1;
}

void shape_int_dnu_dnv(const vector Z, const vector U, const vector V,
        scalar S)
{
    int fn, s, i, j, k, a, b, c;
    face_t *f;
    int ret;
    double TN[3][15][15];
    double n[4][3], z[3][3], u[5][3], v[5][3];
    double *pn = &n[0][0], *pz = &z[0][0], *pu = &u[0][0],
            *pv = &v[0][0], *pTN = &TN[0][0][0];

    if(check_inputs_U(Z, U, V, S) == 0) return;
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        if(f->elems[1] != -1)
            continue;
        if((ret = prep_face_data(f, Z, U, V, pz, pu, pv, pTN, pn)) < 0)
            error_msg("%s: ret == %d", __func__, ret);
        s = ret;

        for(i = 0; i < 3; i++)
        {
            double Val = 0.0;
            double z_dot_ns = mulvec(z[i], n[s]);

            for(j = 0; j < 5; j++)
                for(k = 0; k < 5; k++)
                    for(a = 0; a < 3; a++)
                        for(b = 0; b < 3; b++)
                            for(c = 0; c < 3; c++)
                                Val += z_dot_ns * u[j][a] * v[k][a] * n[s][b] *
                                    n[s][c] * TN[i][j + 5 * b][k + 5 * c];

            SF[f->nodes[i]] += Val;
        }
    }
    done_scalar(S)
}

void shape_int_gradu_gradv(const vector Z, const vector U, const vector V,
        scalar S)
{
    int fn, s, i, j, k, a, b;
    face_t *f;
    int ret;
    double TN[3][15][15];
    double n[4][3], z[3][3], u[5][3], v[5][3];
    double *pn = &n[0][0], *pz = &z[0][0], *pu = &u[0][0], *pv = &v[0][0],
            *pTN = &TN[0][0][0];

    if(check_inputs_U(Z, U, V, S) == 0) return;
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        if(f->elems[1] != -1)
            continue;
        if((ret = prep_face_data(f, Z, U, V, pz, pu, pv, pTN, pn)) < 0)
            error_msg("%s: ret == %d", __func__, ret);
        s = ret;

        for(i = 0; i < 3; i++)
        {
            double Val = 0.0;
            double z_dot_ns = mulvec(z[i], n[s]);

            for(j = 0; j < 5; j++)
                for(k = 0; k < 5; k++)
                    for(a = 0; a < 3; a++)
                        for(b = 0; b < 3; b++)
                            Val += z_dot_ns * u[j][b] * v[k][b] *
                                TN[i][j + 5 * a][k + 5 * a];

            SF[f->nodes[i]] += Val;
        }
    }
    done_scalar(S)
}

void shape_int_curlu_curlv(const vector Z, const vector U, const vector V,
        scalar S)
{
    int fn, s, i, j, k, b, c;
    face_t *f;
    int ret;
    double TN[3][15][15];
    double n[4][3], z[3][3], u[5][3], v[5][3];
    double *pn = &n[0][0], *pz = &z[0][0], *pu = &u[0][0], *pv = &v[0][0],
            *pTN = &TN[0][0][0];

    if(check_inputs_U(Z, U, V, S) == 0) return;
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        if(f->elems[1] != -1)
            continue;
        if((ret = prep_face_data(f, Z, U, V, pz, pu, pv, pTN, pn)) < 0)
            error_msg("%s: ret == %d", __func__, ret);
        s = ret;

        for(i = 0; i < 3; i++)
        {
            double Val = 0.0;
            double z_dot_ns = mulvec(z[i], n[s]);

            for(j = 0; j < 5; j++)
                for(k = 0; k < 5; k++)
                {
                    double SNijk = 0.0;
                    {
                        int a;
                        for(a = 0; a < 3; a++)
                            SNijk += TN[i][j + 5 * a][k + 5 * a];
                    }

                    for(b = 0; b < 3; b++)
                        for(c = 0; c < 3; c++)
                            Val += z_dot_ns * u[j][b] * v[k][c]*
                                ((b == c ? SNijk : 0.0) -
                                TN[i][j + 5 * c][k + 5 * b]);
                    // BUG FIX: 11 Dec 2008: incorrect indices
                    // TN[i][j + 5 * b][k + 5 * c]);
                }

            SF[f->nodes[i]] += Val;
        }
    }
    done_scalar(S)
}

void shape_int_dnu_ncrosscurlv(const vector Z,
        const vector U, const vector V, scalar S)
{
    int fn, s, i, j, k, a, b, c;
    face_t *f;
    int ret;
    double TN[3][15][15];
    double n[4][3], z[3][3], u[5][3], v[5][3];
    double *pn = &n[0][0], *pz = &z[0][0], *pu = &u[0][0], *pv = &v[0][0],
            *pTN = &TN[0][0][0];

    if(check_inputs_U(Z, U, V, S) == 0) return;
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        if(f->elems[1] != -1)
            continue;
        if((ret = prep_face_data(f, Z, U, V, pz, pu, pv, pTN, pn)) < 0)
            error_msg("%s: ret == %d", __func__, ret);
        s = ret;

        for(i = 0; i < 3; i++)
        {
            double Val = 0.0;
            double z_dot_ns = mulvec(z[i], n[s]);

            for(j = 0; j < 5; j++)
                for(k = 0; k < 5; k++)
                    for(a = 0; a < 3; a++)
                    {
                        double nSNijk = 0.0;
                        {
                            int m;
                            for(m = 0; m < 3; m++)
                                nSNijk += n[s][m] * TN[i][j + 5 * a][k + 5 * m];
                        }

                        for(b = 0; b < 3; b++) for(c = 0; c < 3; c++)
                                Val += z_dot_ns * u[j][b] * v[k][c] * n[s][a]*
                                    (n[s][c] * TN[i][j + 5 * a][k + 5 * b]-
                                    (b == c ? nSNijk : 0.0));
                    }

            SF[f->nodes[i]] += Val;
        }
    }
    done_scalar(S)
}

/****************************************************************************/

static
int tensor_L(const face_t *f, const elem_t *T, double *TL, double *n)
/*
 * computes
 *      L[i][j][k+5*c] = int_f \hat{phi}_i \hat{\phi}_j \partial_c \phi_k
 * where
 *      \hat{\phi}_i, i,j=0,1,2 are the P_1 basis on face f
 *      \phi_k, k=0,1,2,3,4 are the P_1-bubble basis in element T
 *      x_c, c=0,1,2 are the spatial variables
 */
{
    int i, j, k, c, s;
    double V, A[4], An[4][3], As_by_3V;

    if(TL == NULL)
        return -1;

    V = ComputeVolume(grid->nodes, T->nodes);
    for(i = 0; i < 4; i++)
    {
        A[i] = ComputeArea(grid->nodes,
                elem_face_nodes(T->nodes, i), n + i * 3);
        for(j = 0; j < 3; j++) An[i][j] = A[i]*(n + i * 3)[j];
    }

    /* f must be a face in T,  s is the local index of f in T */
    s = 0 + 1 + 2 + 3;
    for(i = 0; i < 3; i++)
    {
        for(j = 0; j < 4; j++)
            if(f->nodes[i] == T->nodes[j])
            {
                s -= j;
                break;
            }
        if(j >= 4)
            error_msg("%s: face %ld is not in element %ld.\n", __func__,
                f - grid->faces, T - grid->elems);
    }
    As_by_3V = A[s] / (3 * V);

#define index_L(i,j,k)   ((i)*3*15+(j)*15+(k))

    memset(TL, 0, 3 * 3 * 15 * sizeof (double));
    for(i = 0; i < 3; i++) for(j = 0; j < 3; j++)
        {
            /* case 1: k<4 */
            for(k = 0; k < 4; k++) for(c = 0; c < 3; c++)
                    TL[index_L(i, j, k + 5 * c)] = -As_by_3V * An[k][c]*
                        (1 + (i == j ? 1 : 0)) / 12.0;
            /* case 2: k=4 */
            for(c = 0; c < 3; c++)
                TL[index_L(i, j, 4 + 5 * c)] = -As_by_3V * An[s][c]*
                    (2 + (i == j ? 1 : 0))*64. / 315.0;
        }

#undef index_L

    return s;
}

/****************************************************************************/

static int prep_face_data_L(const face_t *f,
        const vector Z, const scalar P, const vector V,
        double *z, double *p, double *v,
        double *TL, double *n)
{
    int j, i, Tn = f->elems[0];
    elem_t *T = grid->elems + Tn;
    assert(f != NULL && Z != NULL && P != NULL && V != NULL &&
            z != NULL && p != NULL && v != NULL && n != NULL && TL != NULL);

    use_vector(V)
    use_vector(Z)
    use_scalar(P)
    for(j = 0; j < 4; j++)
    {
        (v + j * 3)[0] = VX[T->nodes[j]];
        (v + j * 3)[1] = VY[T->nodes[j]];
        (v + j * 3)[2] = VZ[T->nodes[j]];
    }
    (v + 4 * 3)[0] = VX[grid->NN + Tn];
    (v + 4 * 3)[1] = VY[grid->NN + Tn];
    (v + 4 * 3)[2] = VZ[grid->NN + Tn];

    for(i = 0; i < 3; i++)
    {
        p[i] = PF[f->nodes[i]];
        (z + i * 3)[0] = ZX[f->nodes[i]];
        (z + i * 3)[1] = ZY[f->nodes[i]];
        (z + i * 3)[2] = ZZ[f->nodes[i]];
    }
    done_scalar(P)
    done_vector(Z)
    done_vector(V)

    return tensor_L(f, T, TL, n);
}

/****************************************************************************/

inline static int check_inputs_P(const vector Z, const scalar P,
        const vector V, scalar S)
{
    assert(Z != NULL && S != NULL);
    assert(get_N_vec(Z) == grid->NN);
    assert(get_N_scal(S) == grid->NN);
    clean_scal(S);
    if(P == NULL || V == NULL)
        return 0;
    assert(get_N_scal(P) == grid->NN);
    assert(get_N_vec(V) == fe_ndof(grid, fe_bub));
    return 1;
}

/****************************************************************************/

void shape_int_p_ndnv(const vector Z, const scalar P, const vector V,
        scalar S)
{
    int fn, s, i, j, k, a, c;
    face_t *f;
    int ret;
    static double TL[3][3][15];
    double n[4][3], z[3][3], p[3], v[5][3];
    double *pn = &n[0][0], *pz = &z[0][0], *pp = &p[0], *pv = &v[0][0],
            *pTL = &TL[0][0][0];

    if(check_inputs_P(Z, P, V, S) == 0) return;
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        if(f->elems[1] != -1)
            continue;
        if((ret = prep_face_data_L(f, Z, P, V, pz, pp, pv, pTL, pn)) < 0)
            error_msg("%s: ret == %d", __func__, ret);
        s = ret;

        for(i = 0; i < 3; i++)
        {
            double Val = 0.0;
            double z_dot_ns = mulvec(z[i], n[s]);

            for(j = 0; j < 3; j++)
                for(k = 0; k < 5; k++)
                    for(c = 0; c < 3; c++)
                        for(a = 0; a < 3; a++)
                            Val += z_dot_ns * p[j] * v[k][c] * n[s][c] *
                                n[s][a] * TL[i][j][k + 5 * a];

            SF[f->nodes[i]] += Val;
        }
    }
    done_scalar(S)
}


/****************************************************************************/

double sg_norm; /* the L2 norm */
double sg_norm_inf; /* the Linf norm */
double sg_lambda = 0.1;

vector sg_z, gu0, gu1, gu2, gv0, gv1, gv2, wv1, wv2;

scalar sg_xi, sg_w1, sg_w2, sg_w3;
scalar sg_xi_weak;

fe_matr_p sg_mass;
void eo_sigma(const grid3d_t *grid, unsigned eltype, const int *nodes,
        unsigned **eos);
void local_sigma_mass(const grid3d_t *grid, int el, unsigned type, double ***M);

void make_sg_z(void)
{
    int i, fn, Tn, s;
    face_t *f;
    elem_t *T;
    double A, n[3];

    clean_vec(sg_z);

    use_vector(sg_z)
    for(f = grid->faces + (fn = grid->NFI); fn < grid->NF; fn++, f++)
    {
        if(!_is_face_on_sigma(grid, fn))
            continue;

        Tn = f->elems[0];
        T = grid->elems + Tn;

        // find local index in T of face f
        for(s = 0; s < 4; s++)
            if(T->faces[s] == fn)
                break;

        //++++++++++
        // this checks for consistency of the data (e.g. memory corruption)
        if(s >= 4)
            error_msg("%s: face %d is not in element %d", __func__, fn, Tn);
        //++++++++++

        // get nodes of f in order that gives orintation such that normal is outer
        const int *f_nds = elem_face_nodes(T->nodes, s);

        // compute unit normal n and area A
        A = ComputeArea(grid->nodes, f_nds, n);

        // Compute weights
        double W[3];

        //***  method one: Mean Weighted Equally
        // W[0] = W[1] = W[2] = 1.0;

        //*** method two: Mean Weighted by Area
        // W[0] = W[1] = W[2] = A;

        //*** method three: Mean Weighted by Angle
        // to compute angle we use formula:
        //     sin(alpha) = |a x b| / |a| / |b|
        // note that |a x b| is the area of triangle between a and b
        {
            // compute lengths of edges of f
            double l[3], prod_l = 1.0;
            for(s = 0; s < 3; s++)
            {
                int foo[2] = {f_nds[s], f_nds[ (s + 1) % 3]};
                prod_l *= l[s] = ComputeLength(grid->nodes, foo, NULL);
            }

            for(s = 0; s<3; s++)
                W[s] = asin(A * l[s] / prod_l);
        }

        // to each vertex add weighted normal
        for(s = 0; s < 3; s++)
        {
            sg_zX[f_nds[s]] += W[s] * n[0];
            sg_zY[f_nds[s]] += W[s] * n[1];
            sg_zZ[f_nds[s]] += W[s] * n[2];
        }
    }

    // normalize vertex normals
    for(i = 0; i < grid->NN; i++)
    {
        if(!_is_node_on_sigma(grid, i, sigma_closed))
        {
            sg_zX[i] = sg_zY[i] = sg_zZ[i] = 0.0;
        }
        else
        {
            n[0] = sg_zX[i];
            n[1] = sg_zY[i];
            n[2] = sg_zZ[i];
            if((A = norm(n)) < 1e-10)
                error_msg("%s: normal vector at node %i is 0.\n", __func__, i);
            sg_zX[i] /= A;
            sg_zY[i] /= A;
            sg_zZ[i] /= A;
        }
    }
    done_vector(sg_z)
}

void alloc_sg(void)
{
    int N = fe_ndof(grid, fe_bub);
    sg_z = alloc_vec(grid->NN);
    gu0 = alloc_vec(N);
    gu1 = alloc_vec(N);
    gu2 = alloc_vec(N);
    gv0 = alloc_vec(N);
    gv1 = alloc_vec(N);
    gv2 = alloc_vec(N);
    wv1 = alloc_vec(N);
    wv2 = alloc_vec(N);
    sg_xi = alloc_scal(grid->NN);
    sg_xi_weak = alloc_scal(grid->NN);
    sg_w1 = alloc_scal(grid->NN);
    sg_w2 = alloc_scal(grid->NN);
    sg_w3 = alloc_scal(grid->NN);

#if defined(USE_WEAK_GRADIENT) && (USE_WEAK_GRADIENT != 0)
    sg_mass = fe_matr_alloc(fe_p1, local_sigma_mass, eo_sigma, grid, TRUE, NULL);
#else
    sg_mass = NULL;
#endif
}

void free_sg(void)
{
    free_vec(sg_z);
    free_vec(gu0);
    free_vec(gu1);
    free_vec(gu2);
    free_vec(gv0);
    free_vec(gv1);
    free_vec(gv2);
    free_vec(wv1);
    free_vec(wv2);
    free_scal(sg_xi);
    free_scal(sg_xi_weak);
    free_scal(sg_w1);
    free_scal(sg_w2);
    free_scal(sg_w3);

    fe_matr_free(sg_mass);
}

/****************************************************************************/

static int __get_local_index_of_face(const face_t *f, const elem_t *T)
{
    int i, j, s;
    s = 0 + 1 + 2 + 3;
    for(i = 0; i < 3; i++)
    {
        for(j = 0; j < 4; j++)
            if(f->nodes[i] == T->nodes[j])
            {
                s -= j;
                break;
            }
        if(j >= 4)
            error_msg("%s: face %ld is not in element %ld.\n", __func__,
                f - grid->faces, T - grid->elems);
    }
    return s;
}

void shape_int_scal_scal(const vector Z, const scalar P, const scalar Q,
        scalar S)
{
    int fn, i, j, k;
    face_t *f;
    elem_t *T;
    double A, Val, zn, n[3];

    clean_scal(S);

    use_vector(sg_z)
    use_scalar(P)
    use_scalar(Q)
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        /* is face on boundary */
        if(f->elems[1] != -1)
            continue;
        T = grid->elems + f->elems[0];
        /* find local index of face */
        int s = __get_local_index_of_face(f, T);
        /* compute area of and normal to face */
        A = ComputeArea(grid->nodes, elem_face_nodes(T->nodes, s), n);

        for(i = 0; i < 3; i++)
        {
            Val = 0.0;
            zn = sg_zX[f->nodes[i]] * n[0] + sg_zY[f->nodes[i]] * n[1]
                    + sg_zZ[f->nodes[i]] * n[2];
            for(j = 0; j < 3; j++) for(k = 0; k < 3; k++)
                {
                    int dij = i == j ? 1 : 0;
                    int dik = i == k ? 1 : 0;
                    int djk = k == j ? 1 : 0;
                    int den = 60 - 30 * (dij + dik + djk) + 40 * dij * dik*djk;
                    SF[f->nodes[i]] += zn * PF[f->nodes[j]] * QF[f->nodes[k]] *
                            A / (double) den;
                }
        }
    }
    done_scalar(S)
    done_scalar(Q)
    done_scalar(P)
    done_vector(sg_z)
}

void shape_int_vec_vec(const vector Z, const vector U, const vector V,
        scalar S)
{
    int fn, i, j, k;
    face_t *f;
    elem_t *T;
    double A, Val, zn, n[3], u[3][3], v[3][3];

    clean_scal(S);

    use_vector(U)
    use_vector(V)
    use_vector(Z)
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        /* is face on boundary */
        if(f->elems[1] != -1)
            continue;
        T = grid->elems + f->elems[0];
        /* find local index of face */
        int s = __get_local_index_of_face(f, T);
        /* compute area of and normal to face */
        A = ComputeArea(grid->nodes, elem_face_nodes(T->nodes, s), n);

        for(i = 0; i < 3; i++)
        {
            u[i][0] = UX[f->nodes[i]];
            u[i][1] = UY[f->nodes[i]];
            u[i][2] = UZ[f->nodes[i]];
            v[i][0] = VX[f->nodes[i]];
            v[i][1] = VY[f->nodes[i]];
            v[i][2] = VZ[f->nodes[i]];
        }

        for(i = 0; i < 3; i++)
        {
            Val = 0.0;
            zn = ZX[f->nodes[i]] * n[0] + ZY[f->nodes[i]] * n[1]
                    + ZZ[f->nodes[i]] * n[2];
            for(j = 0; j < 3; j++) for(k = 0; k < 3; k++)
                {
                    int dij = i == j ? 1 : 0;
                    int dik = i == k ? 1 : 0;
                    int djk = k == j ? 1 : 0;
                    int den = 60 - 30 * (dij + dik + djk) + 40 * dij * dik*djk;
                    SF[f->nodes[i]] += zn * A / (double) den *
                            mulvec(u[j], v[k]);
                }
        }
    }
    done_scalar(S)
    done_vector(Z)
    done_vector(V)
    done_vector(U)
}

void shape_int_tens_tens(const vector Z,
        const vector U0, const vector U1, const vector U2,
        const vector V0, const vector V1, const vector V2,
        scalar S)
{
    int fn, i, j, k;
    face_t *f;
    elem_t *T;
    double A, Val, zn, n[3], u[3][3][3], v[3][3][3];

    clean_scal(S);

    use_vector(U0)
    use_vector(U1)
    use_vector(U2)
    use_vector(V0)
    use_vector(V1)
    use_vector(V2)
    use_vector(Z)
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        /* is face on boundary */
        if(f->elems[1] != -1)
            continue;
        T = grid->elems + f->elems[0];
        /* find local index of face */
        int s = __get_local_index_of_face(f, T);
        /* compute area of and normal to face */
        A = ComputeArea(grid->nodes, elem_face_nodes(T->nodes, s), n);

        for(i = 0; i < 3; i++)
        {
            u[i][0][0] = U0X[f->nodes[i]];
            u[i][0][1] = U0Y[f->nodes[i]];
            u[i][0][2] = U0Z[f->nodes[i]];
            u[i][1][0] = U1X[f->nodes[i]];
            u[i][1][1] = U1Y[f->nodes[i]];
            u[i][1][2] = U1Z[f->nodes[i]];
            u[i][2][0] = U2X[f->nodes[i]];
            u[i][2][1] = U2Y[f->nodes[i]];
            u[i][2][2] = U2Z[f->nodes[i]];
            v[i][0][0] = V0X[f->nodes[i]];
            v[i][0][1] = V0Y[f->nodes[i]];
            v[i][0][2] = V0Z[f->nodes[i]];
            v[i][1][0] = V1X[f->nodes[i]];
            v[i][1][1] = V1Y[f->nodes[i]];
            v[i][1][2] = V1Z[f->nodes[i]];
            v[i][2][0] = V2X[f->nodes[i]];
            v[i][2][1] = V2Y[f->nodes[i]];
            v[i][2][2] = V2Z[f->nodes[i]];
        }

        for(i = 0; i < 3; i++)
        {
            Val = 0.0;
            zn = ZX[f->nodes[i]] * n[0] + ZY[f->nodes[i]] * n[1]
                    + ZZ[f->nodes[i]] * n[2];
            for(j = 0; j < 3; j++) for(k = 0; k < 3; k++)
                {
                    int dij = i == j ? 1 : 0;
                    int dik = i == k ? 1 : 0;
                    int djk = k == j ? 1 : 0;
                    int den = 60 - 30 * (dij + dik + djk) + 40 * dij * dik*djk;
                    Val = 0.0;
                    for(int a = 0; a < 3; a++) for(int b = 0; b < 3; b++)
                            Val += u[j][a][b] * v[k][a][b];
                    SF[f->nodes[i]] += zn * Val * A / (double) den;
                }
        }
    }
    done_scalar(S)
    done_vector(Z)
    done_vector(V2)
    done_vector(V1)
    done_vector(V0)
    done_vector(U2)
    done_vector(U1)
    done_vector(U0)
}

void shape_int_tens_dot_n__tens_dot_n
(const vector Z, const vector U0, const vector U1, const vector U2,
        const vector V0, const vector V1, const vector V2, scalar S)
{
    int fn, i, j, k;
    face_t *f;
    double A, Val, zn, n[3], u[3][3], v[3][3];

    clean_scal(S);

    use_vector(U0)
    use_vector(U1)
    use_vector(U2)
    use_vector(V0)
    use_vector(V1)
    use_vector(V2)
    use_vector(Z)
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        /* ------------------------------------------------------------ */
        /*   if face is not on outer boundary we move on                */
        /* ------------------------------------------------------------ */
        if(f->elems[1] != -1)
            continue;

        /* ------------------------------------------------------------ */
        /*   compute face area and normal; normal is outer to element T */
        /* ------------------------------------------------------------ */
        elem_t *T = grid->elems + f->elems[0];
        int s = __get_local_index_of_face(f, T);
        A = ComputeArea(grid->nodes, elem_face_nodes(T->nodes, s), n);

        /* ------------------------------------------------------------ */
        /*   fetch data      */
        /* ------------------------------------------------------------ */
        for(i = 0; i < 3; i++)
        {
            k = f->nodes[i];
            /* this computes tens_dot_n part, where tens has rows U0, U1, U2 */
            u[i][0] = U0X[k] * n[0] + U0Y[k] * n[1] + U0Z[k] * n[2];
            u[i][1] = U1X[k] * n[0] + U1Y[k] * n[1] + U1Z[k] * n[2];
            u[i][2] = U2X[k] * n[0] + U2Y[k] * n[1] + U2Z[k] * n[2];
            /* this computes tens_dot_n part, where tens has rows V0, V1, V2 */
            v[i][0] = V0X[k] * n[0] + V0Y[k] * n[1] + V0Z[k] * n[2];
            v[i][1] = V1X[k] * n[0] + V1Y[k] * n[1] + V1Z[k] * n[2];
            v[i][2] = V2X[k] * n[0] + V2Y[k] * n[1] + V2Z[k] * n[2];
        }

        /* ------------------------------------------------------------ */
        /*   compute the integrals      */
        /* ------------------------------------------------------------ */
        for(i = 0; i < 3; i++)
        {
            Val = 0.0;
            zn = ZX[f->nodes[i]] * n[0] + ZY[f->nodes[i]] * n[1]
                    + ZZ[f->nodes[i]] * n[2];
            for(j = 0; j < 3; j++) for(k = 0; k < 3; k++)
                {
                    int dij = i == j ? 1 : 0;
                    int dik = i == k ? 1 : 0;
                    int djk = k == j ? 1 : 0;
                    int den = 60 - 30 * (dij + dik + djk) + 40 * dij * dik*djk;
                    Val = mulvec(u[j], v[k]);
                    SF[f->nodes[i]] += zn * Val * A / (double) den;
                }
        }
    }
    done_scalar(S)
    done_vector(Z)
    done_vector(V2)
    done_vector(V1)
    done_vector(V0)
    done_vector(U2)
    done_vector(U1)
    done_vector(U0)
}

void shape_int_tens_dot_n__n_cross_vec
(const vector Z, const vector U0, const vector U1, const vector U2,
        const vector V, scalar S)
{
    int fn, i, j, k;
    face_t *f;
    double A, Val, zn, n[3], u[3][3], v[3][3];

    clean_scal(S);

    use_vector(U0)
    use_vector(U1)
    use_vector(U2)
    use_vector(V)
    use_vector(Z)
    use_scalar(S)
    for(fn = 0, f = grid->faces; fn < grid->NF; fn++, f++)
    {
        /* ------------------------------------------------------------ */
        /*   if face is not on outer boundary we move on                */
        /* ------------------------------------------------------------ */
        if(f->elems[1] != -1)
            continue;

        /* ------------------------------------------------------------ */
        /*   compute face area and normal; normal is outer to element T */
        /* ------------------------------------------------------------ */
        elem_t *T = grid->elems + f->elems[0];
        int s = __get_local_index_of_face(f, T);
        A = ComputeArea(grid->nodes, elem_face_nodes(T->nodes, s), n);

        /* ------------------------------------------------------------ */
        /*   fetch data      */
        /* ------------------------------------------------------------ */
        for(i = 0; i < 3; i++)
        {
            k = f->nodes[i];
            /* this computes tens_dot_n part, where tens has rows U0, U1, U2 */
            u[i][0] = U0X[k] * n[0] + U0Y[k] * n[1] + U0Z[k] * n[2];
            u[i][1] = U1X[k] * n[0] + U1Y[k] * n[1] + U1Z[k] * n[2];
            u[i][2] = U2X[k] * n[0] + U2Y[k] * n[1] + U2Z[k] * n[2];
            /* this computes n_cross_vec part, where vec is V */
            v[i][0] = n[1] * VZ[k] - n[2] * VY[k];
            v[i][1] = n[2] * VX[k] - n[0] * VZ[k];
            v[i][2] = n[0] * VY[k] - n[1] * VX[k];
        }

        /* ------------------------------------------------------------ */
        /*   compute the integrals      */
        /* ------------------------------------------------------------ */
        for(i = 0; i < 3; i++)
        {
            Val = 0.0;
            zn = ZX[f->nodes[i]] * n[0] + ZY[f->nodes[i]] * n[1]
                    + ZZ[f->nodes[i]] * n[2];
            for(j = 0; j < 3; j++) for(k = 0; k < 3; k++)
                {
                    int dij = i == j ? 1 : 0;
                    int dik = i == k ? 1 : 0;
                    int djk = k == j ? 1 : 0;
                    int den = 60 - 30 * (dij + dik + djk) + 40 * dij * dik*djk;
                    Val = mulvec(u[j], v[k]);
                    SF[f->nodes[i]] += zn * Val * A / (double) den;
                }
        }
    }
    done_scalar(S)
    done_vector(Z)
    done_vector(V)
    done_vector(U2)
    done_vector(U1)
    done_vector(U0)
}


#include "f3io.h"

// static void shape_int_time_interp(scalar g, scalar work);
static void shape_int_time_old(scalar g, scalar work);
// static void shape_int_time_new(scalar g, scalar work);

static void (*shape_int_time)(scalar g, scalar work) = shape_int_time_old;

#if 0

static void shape_int_time_interp(scalar g, scalar work)
{
    /* ------------------------------------------------------------ */
    /*   load data                                                  */
    /* ------------------------------------------------------------ */

    move_grid();
    load_state_time(state_dir());
    load_adjoint_time(adjoint_dir());

    /* ------------------------------------------------------------ */
    /*   project gradients of u and v onto H1                       */
    /*   ---> gu0->X is the X-derivative of u->X, etc.              */
    /* ------------------------------------------------------------ */
    project_grad_vec(grid, elemU, massU, u, gu0, gu1, gu2, wv1, wv2);
    project_grad_vec(grid, elemU, massU, v, gv0, gv1, gv2, wv1, wv2);

    clean_scal(g);
    int i;

    /* ---------------------------------------------------------- */
    /*   compute shape_int of |grad u|^2                          */
    /* ---------------------------------------------------------- */
    shape_int_tens_tens(sg_z, gu0, gu1, gu2, gu0, gu1, gu2, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] += 0.5*alpha * work->F[i];
    axpy_scal(g, 0.5 * alpha, work);

    /* ---------------------------------------------------------- */
    /*   compute shape_int of  |(n.grad)u|^2                      */
    /* ---------------------------------------------------------- */
    shape_int_tens_dot_n__tens_dot_n(sg_z, gu0, gu1, gu2, gu0, gu1, gu2, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] -= alpha * work->F[i];
    axpy_scal(g, -alpha, work);

    /* ------------------------------------------------------------ */
    /*   compute shape_int of  |curl u|^2                           */
    /* ------------------------------------------------------------ */
    /*     ==>  compute curl u and put it in wv1 */
    clean_vec(wv1);
    int N = fe_ndof(grid, fe_bub);

    use_vector(wv1)
    use_vector(gu0)
    use_vector(gu1)
    use_vector(gu2)
    for(i = 0; i < N; i++)
    {
        wv1X[i] = gu2Y[i] - gu1Z[i];
        wv1Y[i] = gu0Z[i] - gu2X[i];
        wv1Z[i] = gu1X[i] - gu0Y[i];
    }
    done_vector(gu2)
    done_vector(gu1)
    done_vector(gu0)
    done_vector(wv1)

    /*     ==>  compute shape_int of |wv1|^2  */
    shape_int_vec_vec(sg_z, wv1, wv1, work);
    // for(i=0;i<grid->NN;i++)
    //    g->F[i] += 0.5*beta * work->F[i];
    axpy_scal(g, 0.5 * beta, work);

    /* ------------------------------------------------------------ */
    /*   compute shape_int of (n.grad)u . (n x curl u)              */
    /* ------------------------------------------------------------ */
    /*  !!! this assumes that curl u is in wv1 */
    shape_int_tens_dot_n__n_cross_vec(sg_z, gu0, gu1, gu2, wv1, work);
    // for(i=0;i<grid->NN;i++)
    //    g->F[i] += 0.5*beta * work->F[i];
    axpy_scal(g, beta, work);

    /* ---------------------------------------------------------- */
    /*   compute shape_int of  (n.grad)u.(n.grad)v                */
    /* ---------------------------------------------------------- */
    shape_int_tens_dot_n__tens_dot_n(sg_z, gu0, gu1, gu2, gv0, gv1, gv2, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] += 1.0/Re * work->F[i];
    axpy_scal(g, 1.0 / GetRe(), work);


    return;
}
#endif

static void shape_int_time_old(scalar g, scalar work)
{
    // int i;

    move_grid();
    load_state_time(state_dir());
    load_adjoint_time(adjoint_dir());

    clean_scal(g);

    shape_int_dnu_dnv(sg_z, u, v, work);
    // for(i=0;i<grid->NN;i++)
    //    g->F[i] += 1.0/Re * work->F[i];
    axpy_scal(g, 1.0 / GetRe(), work);

    shape_int_dnu_dnv(sg_z, u, u, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] -= alpha * work->F[i];
    axpy_scal(g, -alpha, work);

    shape_int_dnu_ncrosscurlv(sg_z, u, u, work);
    // for(i=0;i<grid->NN;i++)
    //    g->F[i] += beta * work->F[i];
    axpy_scal(g, +beta, work);

    shape_int_gradu_gradv(sg_z, u, u, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] += 0.5*alpha * work->F[i];
    axpy_scal(g, 0.5 * alpha, work);

    shape_int_curlu_curlv(sg_z, u, u, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] += 0.5*beta * work->F[i];
    axpy_scal(g, 0.5 * beta, work);

    //shape_int_p_ndnv(sg_z, q, u, work);
    // for(i=0;i<grid->NN;i++)
    //    g->F[i] -=  work->F[i];
    //axpy_scal(g, -1.0, work);

    return;
}

#if 0

static void shape_int_time_new(scalar g, scalar work)
{
    // int i;

    move_grid();
    load_state_time(state_dir());
    load_adjoint_time(adjoint_dir());

    clean_scal(g);

    shape_int_gradu_gradv(sg_z, u, u, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] += 0.5*alpha * work->F[i];
    axpy_scal(g, 0.5 * alpha, work);

    shape_int_curlu_curlv(sg_z, u, u, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] += 0.5*beta * work->F[i];
    axpy_scal(g, 0.5 * beta, work);

    shape_int_gradu_gradv(sg_z, u, v, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] += 1.0/Re * work->F[i];
    axpy_scal(g, 1.0 / GetRe(), work);

    shape_int_p_ndnv(sg_z, p, v, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] +=  work->F[i];
    axpy_scal(g, 1.0, work);

    shape_int_dnu_dnv(sg_z, u, v, work);
    // for(i=0;i<grid->NN;i++)
    //     g->F[i] -= 1.0/Re * work->F[i];
    axpy_scal(g, -1.0, work);

    return;
}
#endif 

static double compute_sg_norm_inf(void)
{
    sg_norm_inf = 0.0;
    use_scalar(sg_xi);
    for(int i = grid->NNI; i < grid->NN; i++)
        if(_is_node_on_sigma(grid, i, sigma_open))
            sg_norm_inf = fmax(sg_norm_inf, fabs(sg_xiF[i]));
    done_scalar(sg_xi);
    return sg_norm_inf;
}

/*
static double compute_sg_norm(void)
{
    double norm = 0.0;

    use_scalar(sg_xi)
    use_vector(sg_z)
    for (int fn = grid->NFI; fn < grid->NF; fn++)
    {
        face_t *f = grid->faces + fn;
        double A = ComputeArea(grid->nodes, f->nodes, NULL);
        double xi[3], z[3][3];

        for (int i = 0; i < 3; i++)
        {
            xi[i] = sg_xiF[f->nodes[i]];
            z[i][0] = sg_zX[f->nodes[i]];
            z[i][1] = sg_zY[f->nodes[i]];
            z[i][2] = sg_zZ[f->nodes[i]];
        }

        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                norm += xi[i] * xi[j] * A / ((i == j) ? 6.0 : 12.0) *
                mulvec(z[i], z[j]);
    }
    done_vector(sg_z)
    done_scalar(sg_xi)
    if (norm < SMALLF)
        if (norm < -1e-10)
            error_msg("%s: norm is negative (%g)\n", __func__, norm);
        else
            return 0.0;
    else
        return sqrt(norm);
}
 */

int compute_shape_gradient(void)
{

    LOG_MPI_WTIME_START;

    char fname[FILENAME_MAX];
    // int i;
    TurnLogOff();

    clean_scal(sg_xi);

    /* this loop implements the composite trapezoidal rule */
    curr_ti = 0;
    shape_int_time(sg_w2, sg_w3);
    for(curr_ti = 1; curr_ti <= total_ti; curr_ti++)
    {
        copy_scal(sg_w2, sg_w1);
        shape_int_time(sg_w2, sg_w3);
        // for( i=0;i<grid->NN;i++)
        //     sg_xi->F[i] += TStep*0.5*(sg_w1->F[i]+sg_w2->F[i]);
        axpy_scal(sg_xi, 0.5 * TStep, sg_w1);
        axpy_scal(sg_xi, 0.5 * TStep, sg_w2);
    }

#if defined(USE_WEAK_GRADIENT) && (USE_WEAK_GRADIENT != 0)
    // BUG FIX: November 2008: invert the sigma-mass matrix on xi!!!
    sprintf(fname, "%s/xi_strong.txt", root_dir());
    print_scal(sg_xi, fname);
    copy_scal(sg_xi, sg_xi_weak);

    fe_matr_assemble(grid, 1, &sg_mass);
    copy_scal(sg_xi, sg_w1);
    fe_matr_solve_scalar(sg_mass, sg_xi, sg_w1, sg_w2, 0.0);
    // BUG FIX: done
#endif

#if (COST_FUNC_SQUARE == 0)
    // for( i=0;i<grid->NN;i++)
    //     sg_xi->F[i] /= total_cost;
    scale_scal(sg_xi, 1.0 / total_cost);
#endif    

    /* -------------------------------------------------------------------- */

#if (SHAPE_GRAD_SMOOTH != 0)
    copy_scal(sg_xi, sg_w1);
    clean_scal(sg_xi);
    clean_scal(sg_w2);

    use_scalar(sg_w1)
    use_scalar(sg_w2)
    use_scalar(sg_xi)
    for(i = 0; i < grid->NF; i++)
    {
        face_t *f = grid->faces + i;
        if(f->elems[1] != -1) continue;
        sg_xiF[f->nodes[0]] += sg_w1F[f->nodes[1]] + sg_w1F[f->nodes[2]];
        sg_xiF[f->nodes[1]] += sg_w1F[f->nodes[2]] + sg_w1F[f->nodes[0]];
        sg_xiF[f->nodes[2]] += sg_w1F[f->nodes[0]] + sg_w1F[f->nodes[1]];
        sg_w2F[f->nodes[0]] += 2.0;
        sg_w2F[f->nodes[1]] += 2.0;
        sg_w2F[f->nodes[2]] += 2.0;
    }
    for(i = 0; i < grid->NN; i++)
    {
        if(_is_node_on_sigma(grid, i, sigma_open))
            sg_xiF[i] = 0.5 * sg_xiF[i] / sg_w2F[i] + 0.5 * sg_w1F[i];
        else
            sg_xiF[i] = 0.0;
    }
    done_scalar(sg_xi)
    done_scalar(sg_w2)
    done_scalar(sg_w1)
#endif    

    sprintf(fname, "%s/xi.txt", root_dir());
    print_scal(sg_xi, fname);

    sprintf(fname, "%s/Z.txt", root_dir());
    print_vec(sg_z, fname);

    /****************************************************************/


    TurnLogOn();

    LOG_MPI_WTIME_STOP;

    // sg_norm = compute_sg_norm();
    extern double norm_sigma_scalar_zz(grid3d_t *grid, const scalar s1,
            const scalar s2);
    sg_norm_inf = compute_sg_norm_inf();
    sg_norm = norm_sigma_scalar_zz(grid, sg_xi, NULL);
    PrintLog("       %10s  %-20s = %.8g\n", "sg_norm_inf", "", sg_norm_inf);
    PrintLog("       %10s   %-20s = %.8g\n", "shape_grad", "", sg_norm);
    PrintLog("       %10s   %-20s = %.8g\n", "total_cost", "", total_cost);
    PRINTF("\r       %10s   %-20s = %.8g\n", "shape_grad", "", sg_norm);
    PRINTF("\r       %10s  %-20s = %.8g\n", "sg_norm_inf", "", sg_norm_inf);

    if(total_cost < 1e-6)
        return TRUE;
    else
        return FALSE;

}


/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/
/****************************************************************************/

/****************************************************************************/

void make_test_z(int c, double b)
{
    double val = 2.0 * b - 1.0;

    if(c < 0 || c > 2 || (fabs(b - 0.0) > 1e-5 && fabs(b - 1.0) > 1e-5))
        error_msg("%s: mambo #5\n", __func__);

    clean_vec(sg_z);

    use_vector(sg_z)
    for(int i = grid->NNI; i < grid->NN; i++)
    {
        node_t *N = grid->nodes + i;
        if(fabs(N->coords[c] - b) < 0.001 * grid->MAXH)
            switch(c)
            {
                case 0: sg_zX[i] = val;
                    break;
                case 1: sg_zY[i] = val;
                    break;
                case 2: sg_zZ[i] = val;
                    break;
            }
    }
    done_vector(sg_z)
}

void test_shape_gradient(void)
{
    int c = 0;
    double b = 1.0;
    make_test_z(c, b);

    shape_int_p_ndnv(sg_z, p, u, sg_w1);
    double val;
    // val = 0.0; 
    // for(int i=0;i<grid->NN;i++) val += sg_w1->F[i];
    val = sum_scal(sg_w1);

    PRINTF(" c = %d, b = %g, val = %g\n", c, b, val);

}

/****************************************************************************/

/* the Sigma mass matrix */


int _is_face_on_sigma(const grid3d_t *grid, int fn)
{
    if(fn < grid->NFI)
        return FALSE;
    face_t *f = grid->faces + fn;
    for(int s = 0; s < 3; s++)
        if(!_is_node_on_sigma(grid, f->nodes[s], sigma_closed))
            return FALSE;
    return TRUE;
}

int _is_node_on_sigma(const grid3d_t *grid, int nn, int open_closed)
{
    /* declared in soah_common.c; Do not expose to everyone */
    extern int on_chbr;
    unsigned on_not_sigma = ~on_chbr;
    unsigned node_on_sigma = grid->nodes[nn].on_bdry & ~on_not_sigma;
    if(open_closed == sigma_open)
    {
        // this excludes the edge of sigma
        return node_on_sigma && (!(grid->nodes[nn].on_bdry & on_not_sigma));
    }
    else
    {
        // this includes the edge of sigma
        return node_on_sigma;
    }

    // this includes the edge of sigma
    // return (grid->nodes[nn].on_bdry & on_chbr);

}

void local_mass_face(const grid3d_t *grid, int fn, double ***M)
{
    static double A[9], *AA[3] = {A, A + 3, A + 6};
    if(M == NULL)
        return;
    else
        *M = AA;
    memset(A, 0, sizeof (A));

    face_t *f = grid->faces + fn;
    int i, j;
    double Area = ComputeArea(grid->nodes, f->nodes, NULL);
    for(i = 0; i < 3; i++)
        for(j = 0; j < 3; j++)
            AA[i][j] = Area / ((i == j) ? 6.0 : 12.0);
}

void local_mass_face_zz(const grid3d_t *grid, int fn, double ***M)
{
    local_mass_face(grid, fn, M);
    int i, j;
    double z[3][3];
    face_t *f = grid->faces + fn;
    use_vector(sg_z);
    for(i = 0; i < 3; i++)
    {
        z[i][0] = sg_zX[f->nodes[i]];
        z[i][1] = sg_zY[f->nodes[i]];
        z[i][2] = sg_zZ[f->nodes[i]];
    }
    done_vector(sg_z);
    for(i = 0; i < 3; i++)
        for(j = 0; j < 3; j++)
            (*M)[i][j] *= mulvec(z[i], z[j]);
}

void local_sigma_mass(const grid3d_t *grid, int el, unsigned type, double ***M)
{
    static double A[16], *AA[4] = {A, A + 4, A + 8, A + 12};
    if(M == NULL)
        return;
    else
        *M = AA;
    memset(A, 0, sizeof (A));

    elem_t *T = grid->elems + el;
    int s;
    // int node_used[4] = {0, 0, 0, 0};
    for(s = 0; s < 4; s++)
    {
        if(!_is_face_on_sigma(grid, T->faces[s]))
            continue;
        double **fM;
        local_mass_face_zz(grid, T->faces[s], &fM);

        face_t *f = grid->faces + T->faces[s];
        int *face_nodes = elem_face_nodes_local(T->nodes, f->nodes);
        if(face_nodes == NULL)
            error_msg("%s: face is not in element.\n", __func__);
        int i, j, ei, ej;
        for(i = 0; i < 3; i++)
        {
            ei = face_nodes[i];
            // node_used[ei] = 1;
            for(j = 0; j < 3; j++)
            {
                ej = face_nodes[j];
                AA[ei][ej] += fM[i][j];
            }
        }
    }
    /*
    for (s = 0; s < 4; s++)
    {
        if (!node_used[s])
            AA[s][s] = 1.0;
    }
     */
    return;
}

void test_vector_sigma(const grid3d_t *grid, vector u, scalar v)
{
    assert(grid != NULL);
    clean_scal(v);
    use_vector(u)
    use_scalar(v)
    for(int fn = 0; fn < grid->NF; fn++)
    {
        if(!_is_face_on_sigma(grid, fn))
            continue;
        double **fM;
        int i, j;
        double z[3][3], uu[3][3];
        face_t *f = grid->faces + fn;
        local_mass_face(grid, fn, &fM);
        use_vector(sg_z)
        for(i = 0; i < 3; i++)
        {
            z[i][0] = sg_zX[f->nodes[i]];
            z[i][1] = sg_zY[f->nodes[i]];
            z[i][2] = sg_zZ[f->nodes[i]];
            uu[i][0] = uX[f->nodes[i]];
            uu[i][1] = uY[f->nodes[i]];
            uu[i][2] = uZ[f->nodes[i]];
        }
        done_vector(sg_z)
        for(i = 0; i < 3; i++)
            for(j = 0; j < 3; j++)
            {
                vF[f->nodes[i]] += fM[i][j] * mulvec(z[i], uu[j]);
            }
    }
    done_scalar(v)
    done_vector(u)
}

void eo_sigma(const grid3d_t *grid, unsigned eltype, const int *nodes,
        unsigned **eos)
{
    static unsigned eo[MAX_LDOFS];
    if(eos)
        *eos = eo;
    else
        return;
    if(eltype != fe_p1)
        error_msg("%s: the sigma mass matrix is only "
            "available in p1.\n", __func__);
    for(int i = 0; i < 4; i++)
    {
        // pass sigma_open to leave edge fixed (i.e. eo_bc) 
        if(_is_node_on_sigma(grid, nodes[i], sigma_open))
            eo[i] = eo_add;
        else
            eo[i] = eo_bc;
    }

}

/****************************************************************************/


double norm_sigma_vector(grid3d_t *grid, const vector v1, const vector v2)
{
    double norm = 0.0;
    assert(grid);
    assert(v1);
    vector V2 = v2 == NULL ? v1 : v2;
    use_vector(v1)
    use_vector(V2)
    for(int fn = 0; fn < grid->NF; fn++)
    {
        if(!_is_face_on_sigma(grid, fn))
            continue;
        double **fM;
        int i, j;
        double dv[3][3];
        face_t *f = grid->faces + fn;
        local_mass_face(grid, fn, &fM);
        for(i = 0; i < 3; i++)
        {
            dv[i][0] = v1X[f->nodes[i]];
            dv[i][1] = v1Y[f->nodes[i]];
            dv[i][2] = v1Z[f->nodes[i]];
            if(v1 == V2)
                continue;
            dv[i][0] -= V2X[f->nodes[i]];
            dv[i][1] -= V2Y[f->nodes[i]];
            dv[i][2] -= V2Z[f->nodes[i]];
        }
        for(i = 0; i < 3; i++)
            for(j = 0; j < 3; j++)
                norm += mulvec(dv[i], dv[j]) * fM[i][j];
    }
    done_vector(V2)
    done_vector(v1)
    assert(norm > -SMALLF * 1e3);
    return sqrt(fabs(norm));
}

double norm_sigma_scalar_zz(grid3d_t *grid, const scalar s1, const scalar s2)
{
    double norm = 0.0;
    assert(grid);
    assert(s1);
    scalar S2 = (s2 == NULL) ? s1 : s2;
    use_scalar(s1)
    use_scalar(S2)
    use_vector(sg_z)
    for(int fn = 0; fn < grid->NF; fn++)
    {
        if(!_is_face_on_sigma(grid, fn))
            continue;
        double **fM;
        int i, j;
        double ds[3], z[3][3];
        face_t *f = grid->faces + fn;
        local_mass_face(grid, fn, &fM);
        for(i = 0; i < 3; i++)
        {
            z[i][0] = sg_zX[f->nodes[i]];
            z[i][1] = sg_zY[f->nodes[i]];
            z[i][2] = sg_zZ[f->nodes[i]];
            ds[i] = s1F[f->nodes[i]];
            if(s1 == S2)
                continue;
            ds[i] -= S2F[f->nodes[i]];
        }
        for(i = 0; i < 3; i++)
            for(j = 0; j < 3; j++)
                norm += mulvec(z[i], z[j]) * ds[i] * ds[j] * fM[i][j];
    }
    done_vector(sg_z)
    done_scalar(S2)
    done_scalar(s1)
    assert(norm > -SMALLF * 1e3);
    return sqrt(fabs(norm));
}

double norm_sigma_vecscal_z(grid3d_t *grid, const vector v1, const scalar s2)
{
    double norm = 0.0;
    assert(grid);
    assert(v1);
    assert(s2);
    use_vector(v1)
    use_scalar(s2)
    use_vector(sg_z)
    for(int fn = 0; fn < grid->NF; fn++)
    {
        if(!_is_face_on_sigma(grid, fn))
            continue;
        double **fM;
        int i, j;
        double dv[3][3];
        face_t *f = grid->faces + fn;
        local_mass_face(grid, fn, &fM);
        for(i = 0; i < 3; i++)
        {
            dv[i][0] = v1X[f->nodes[i]];
            dv[i][1] = v1Y[f->nodes[i]];
            dv[i][2] = v1Z[f->nodes[i]];
            dv[i][0] -= s2F[f->nodes[i]] * sg_zX[f->nodes[i]];
            dv[i][1] -= s2F[f->nodes[i]] * sg_zY[f->nodes[i]];
            dv[i][2] -= s2F[f->nodes[i]] * sg_zZ[f->nodes[i]];
        }
        for(i = 0; i < 3; i++)
            for(j = 0; j < 3; j++)
                norm += mulvec(dv[i], dv[j]) * fM[i][j];
    }
    done_vector(sg_z)
    done_scalar(s2)
    done_vector(v1)
    assert(norm > -SMALLF * 1e3);
    return sqrt(fabs(norm));
}





